#include <iostream>
#include <ctime>

#include "Fatworm.h"
#include "Utils.h"
#include "FatwormCommands.h"

using namespace std;

Fatworm::Fatworm(ShipInfo x)
{
	update(x);
	attackTarget = -1;
	islandTarget = -1;
	rotAngel = 0;
	attackIdx = -1;
	attackStyle = 0;
	keepDis = 300;
	updatedMov = false;
	updatedAtt = false;
	updatedRot = false;
	aimed = false;
	stopMov = false;
	stopRot = false;
	justMov = false;
	curCommand = IDLE;
	blockCount = 0;
	nextSeq = false;
	totallyStopSince = 0;
}


Fatworm::~Fatworm(void)
{
}

void Fatworm::update(ShipInfo x) {
	if ((dir - TPoint(x.DirectionX, x.DirectionY)).norm() > 0.001
		|| (pos - TPoint(x.PositionX, x.PositionY)).norm() > 0.001)
		totallyStopSince = nowTime();

	ID = x.ID;
	team = x.Faction;
	health = x.Armor;
	cd[0] = x.CooldownRemain[0];
	cd[1] = x.CooldownRemain[1];
	speed = x.CurrentSpeed;
	dir = TPoint(x.DirectionX, x.DirectionY);
	isBlocked = x.IsBlocked;
	isMoving = x.IsMoving;
	isRotating = x.IsRotating;
	pos = TPoint(x.PositionX, x.PositionY);
	r = x.Rotation;
	axisSpeed = TPoint(x.VelocityX, x.VelocityY);
	dead = false;
}

void Fatworm::takeIsland( int x )
{
	if (curCommand == TAKE_ISLAND && islandTarget == x)
		return;
	if (curCommand == SEQ_MOV && bakCommand == TAKE_ISLAND) {
		if (islandTarget == x) return;
		else {
			bakCommand = -1;
		}
	}
	moveToPoint(islands[x].pos);
	islandTarget = x;
	curCommand = TAKE_ISLAND;
}

TPoint Fatworm::avoidBlock(TPoint x, bool seqMov) {
	int minIdx = -1;
	float minDis = FLT_MAX;
	float radius = IslandBoundingRadius + ShipBoundingRadius + 5;

	for (unsigned int i=0; i<islands.size(); ++i) {
		float dis = (islands[i].pos - pos).dis(x - pos);
// 		cout << "dis: " << dis << " " << (islands[i].pos - pos)
// 			<< " " << (x - pos) << endl;
// 		cout << (islands[i].pos - pos).norm() << " " << (x-pos).norm() << endl;
		if ((dis < radius) &&
			((islands[i].pos-pos).norm()<(x-pos).norm() - radius)
			) {
			if (minDis > (islands[i].pos - pos).norm()) {
				minDis = (islands[i].pos - pos).norm();
				minIdx = i;
			}
		}
	}

	cout << "in avoid block: " << minIdx << endl;
	if (minIdx!=-1) {
		float c = (islands[minIdx].pos - pos).norm();
		float changeR = asin((radius+25)/c)*180/M_PI;
		cout << "c: " << c << " changeR: " << changeR << endl;
		if ((x-pos).dot(islands[minIdx].pos-pos) < 0) {
			changeR = -changeR;
		}
		if (seqMov) {
			bakMoveTarget = x;
			bakCommand = curCommand;
			curCommand = SEQ_MOV;
			movSequence.push_back(new MoveCommand(*this, x));
			nextSeq = false;
		}
		cout << (islands[minIdx].pos-pos) << " "
			<< (islands[minIdx].pos-pos).rotate(changeR)+pos << endl;
		return ((islands[minIdx].pos-pos).rotate(changeR)+pos);
	}
	return x;
}

void Fatworm::tracking( void )
{
	if (curCommand == SEQ_MOV && bakCommand == TRACKING)
		return;
	keepDis = 320;
	TPoint nextDir(0,0);
	float minDis = FLT_MAX;
	int minIdx;
	bool runAway = false;
	for (unsigned int i = 0; i < enemies.size(); ++i) {
		float dis = (enemies[i].pos - pos).norm();
		if (dis < keepDis) {
			nextDir = nextDir + enemies[i].pos - pos;
			runAway = true;
		}
		if (!runAway && dis < minDis) {
			minDis = dis;
			minIdx = i;
		}
	}
	if (runAway) {
		nextDir = - nextDir;
		nextDir = avoidBlock(nextDir.normizeTo(100), false);
		moveToPoint(nextDir);
	}
	else {
		moveToPoint(enemies[minIdx].pos);
	}
	curCommand = TRACKING;
}

void Fatworm::attack( int idx, int style )
{
	curCommand = ATTACK;
	setAttackTarget(idx);
	attackStyle = style;
}

void Fatworm::rush(void) {
	moveToPoint(enemies[attackIdx].pos);
}

void Fatworm::circleAttack(void) {
	keepDis = 100;
	justMov = true;
	setRotation(45);
}

void Fatworm::rushAttack(void)
{
	TPoint enemyMe = enemies[attackIdx].pos - pos; 
	float dis = enemyMe.norm();
	cout << "dir: " << dir << " " << r << endl;
	cout << "dis: " << dis << endl;

	float enemyMeR = enemyMe.angel();
	cout << "enemyMe: " << enemyMe << " " << enemyMeR << endl;

	float tarR1 = fitAngel(enemyMeR-45);
	float tarR2 = fitAngel(enemyMeR+45);

	cout << "tarR1: " << tarR1 << endl;
	cout << "tarR2: " << tarR2 << endl;

	float time1, time2;
	time1 = max(cd[0], fabs(fitAngel(r - tarR1)) / AngularRate);
	time2 = max(cd[1], fabs(fitAngel(r - tarR2)) / AngularRate);

	cout << "time1: " << time1 << endl;
	cout << "time2: " << time2 << endl;

	justMov = true;
	if (time1 < time2) {
		setRotation(tarR1 - r);
		cout << "choose 1: " << (tarR1-r) << endl;
// 		TPoint tar = enemyMe.rotate(-45).normizeTo(100);
// 		cout << "choose 1: " << tar << " " << tar.angel() << endl;
// 		moveToPoint(enemyMe.rotate(-45).normizeTo(100));
	}
	else {
		setRotation(tarR2 - r);
		cout << "choose 2: " << (tarR2-r) << endl;
// 		TPoint tar = enemyMe.rotate(45).normizeTo(100);
// 		cout << "choose 2: " << tar << " " << tar.angel() << endl;
// 		moveToPoint(enemyMe.rotate(45).normizeTo(100));
	}
	if (enemyMe.norm() < RUSH_DIS)
		attackStyle = RUSH;
}

void Fatworm::cancelCommand(void) {
	curCommand = IDLE;
	stopMov = true;
}

void Fatworm::act( OSInterface* const system )
{
	if (stopRot) {
		system->StopRotating(ID);
		stopRot = false;
	}
	if (stopMov) {
		system->Stop(ID);
		stopMov = false;
	}
	if (justMov) {
		system->StartMoving(ID);
		justMov = false;
	}
	if (updatedMov) {
		cout << "ID: " << ID << " CurCommand: " << curCommand << endl;
		system->MoveTo(ID, moveTarget.x, moveTarget.y);
		updatedMov = false;
	}
	if (updatedAtt) {
		cout << "ID: " << ID << " CurCommand: " << curCommand << endl;
		system->Attack(ID, attackTarget);
		updatedAtt = false;
	}
	if (updatedRot) {
		cout << "ID: " << ID << " CurCommand: " << curCommand << endl;
		system->StartRotating(ID, tarAngel);
		updatedRot = false;
	}
}

void Fatworm::regularCheck( void )
{
	if (aimed) {
		int outRangeState = outOfRange();
		if (outRangeState > 0 && curCommand != ATTACK)
			aimed = false;
	}

	switch (curCommand) {
	case IDLE:
		meetNearestAlli();
		break;
	case TAKE_ISLAND:
		if (islands[islandTarget].team == team) {
			curCommand = IDLE;
		}
		break;
	case ATTACK:
		switch (attackStyle) {
		case CIRCLE_ATTACK:
			circleAttack();
			break;
		case RUSH_ATTACK:
			rushAttack();
			break;
		case RUSH:
			rush();
			break;
		}
		break;
	case TRACKING:
		break;
	case SEQ_MOV:
		if (nextSeq) {
			cout << movSequence.size() << " "
				<< movSequence[movSequence.size() - 1]->typeName() << endl;
			movSequence[movSequence.size() - 1]->execute();
			delete movSequence[movSequence.size() - 1];
			movSequence.pop_back();
			nextSeq = false;
			if (movSequence.size() == 0)
				curCommand = bakCommand;
		}
		else {
			cout << "onPosition: " << onPosition << " "
				<< "tarAngel-r: " << (tarAngel - r) << " " 
				<< "nowTime-totalStop: " << nowTime() - totallyStopSince
				<< endl;
			if (onPosition)
				nextSeq = true;
			else if (fabs(tarAngel - r) < 5)
				nextSeq = true;
			else if (nowTime() - totallyStopSince > 10) {
				nextSeq = true;
			}

		}
		break;
	case HOLD:
		break;
	}

	checkBlock();

	if (curCommand!=ATTACK && !aimed) {
		float minDis = FLT_MAX;
		int minIdx = -1;
		for (unsigned int i=0; i<enemies.size(); ++i) {
			float dis = (pos - enemies[i].pos).norm();
			if (dis < CannonRange && dis < minDis) {
				minDis = dis;
				minIdx = i;
			}
		}
		if (minIdx != -1) {
			setAttackTarget(minIdx);
		}
	}
}

void Fatworm::setAttackTarget( int x )
{
	if (aimed == true && attackTarget == enemies[x].ID)
		return;
	attackIdx = x;
	attackTarget = enemies[x].ID;
	updatedAtt = true;
	aimed = true;
}

void Fatworm::meetAlliance( int idx )
{
	vector<Fatworm> ours = *(this->ours);
	if (ours[idx].curCommand != IDLE) {
		copyCommand(ours[idx]);
	}
	else
		moveToPoint(ours[idx].pos);
}

void Fatworm::meetNearestAlli( void )
{
	vector<Fatworm> ours = *(this->ours);
	if (ours.size() <= 1) return;

	float mindis=FLT_MAX;
	int nearestAlliance = -1;
	for (unsigned int j=0; j<ours.size(); ++j) {
		if (ours[j].ID == ID) continue;
		float dis = (pos - ours[j].pos).norm();
		if (dis > 0) continue;
		if (dis < mindis) {
			mindis = dis;
			nearestAlliance = j;
		}
	}
// 	if (nearestAlliance == -1) {
// 		string x;
// 		x = "no nearest alliance";
// 		cout << "bug " << x << endl;
// 		for (unsigned int j=0; j<ours.size(); ++j) {
// 			TPoint y = ours[ID].pos - ours[j].pos;
// 			cout << y << " "
// 				<< y.norm() << " " 
// 				<< (ours[ID].pos - ours[j].pos).norm()
// 				<< endl;
// 		}
// 	}
	if (nearestAlliance != -1)
		meetAlliance(nearestAlliance);
}

int Fatworm::outOfRange( void )
{
	if (attackIdx == -1) {
		//totally miss
		return 2;
	}
	if ((enemies[attackIdx].pos-pos).norm() > CannonRange)
		return 1;
	return 0;
}

void Fatworm::moveToPoint( TPoint t )
{
	moveTarget = fitEdge(t);
	updatedMov = true;
	onPosition = (moveTarget-pos).norm() < 25;
}

void Fatworm::updateInfo( void )
{
	//update attack index
	attackIdx = -1;
	for (unsigned int i = 0; i < enemies.size(); ++i) {
		if (enemies[i].ID == attackTarget) {
			attackIdx = i;
			break;
		}
	}

	onPosition = (moveTarget-pos).norm() < 25;
}

void Fatworm::setRotation( float r )
{
	updatedRot = true;
	tarAngel = fitAngel(this->r + r);
	rotAngel = r;
}

void Fatworm::copyCommand( const Fatworm& ship )
{
	switch (curCommand) {
	case TAKE_ISLAND:
	case TRACKING:
		break;
	}
	moveToPoint(ship.moveTarget);
}

void Fatworm::checkBlock( void )
{
	if (isBlocked)
		++blockCount;
	else
		blockCount = 0;
// 	if (ID == 4)
// 	cout << "block count: " << blockCount << " " 
// 		<< "ID: " << ID << " "
// 		<< "command: " << curCommand << " "
// 		<< "speed: " << speed << " " 
// 		<< "isBlocked: " << isBlocked
// 		<< endl;
	if (isBlocked && (curCommand != SEQ_MOV || nowTime() - totallyStopSince > 2)) {

		cout << "blocked !" << endl;
		bakMoveTarget = moveTarget;
		bakCommand = curCommand;

		movSequence.push_back(new MoveCommand(*this, moveTarget));

		curCommand = SEQ_MOV;

		movSequence.push_back(new RotateMoveCommand(*this, -45, 100));
		movSequence.push_back(new RotateMoveCommand(*this, -45, 100));
		movSequence.push_back(new RotateMoveCommand(*this, -45, 100));
		movSequence.push_back(new RotateCommand(*this, 180));
// 		movSequence.push_back(StopCommand(*this));
		nextSeq = true;
	}
	else {
		TPoint unitDir = dir.normizeTo(1);
		if ((!isRotating) && willBlock(pos + unitDir*100)) {
			setRotation(90);
			bakCommand = curCommand;
			bakMoveTarget = moveTarget;
			curCommand = SEQ_MOV;
		}
	}
}

bool Fatworm::willBlock( TPoint x )
{
	return false;
	for (unsigned int i=0; i<islands.size(); ++i) {
		if ((x - islands[i].pos).norm() < IslandBoundingRadius + ShipBoundingRadius)
			return true;
	}
	return false;
}

void Fatworm::hold( void )
{
	if (curCommand == HOLD && (!isMoving) && (!isRotating))
		return;
	curCommand = HOLD;
	stopMov = true;
}

void Fatworm::hold( TPoint x )
{
	if (curCommand == HOLD && (moveTarget-x).norm() < 0.001)
		return;
	curCommand = HOLD;
	moveToPoint(x);
}
